#include<stdio.h>

#include<Mesh_IO/Mesh_IO.h>

#include<Mesh/libcell_iterator.h>

#include<tool/libcell_tools_view.h>
#include<tool/viewer_tools.h>


#include<time.h>
#include<libcell_macros.h>
#include<Arcroll.h>
#include<tools_heap.h>
#define Matrix4x4 Viewer_Matrix4x4_


static void test_show_mesh_cells_from_node(Viewer_World*vw,Node*node)
{
    Node* n=vw->create_something(vw,"Faces");
    Viewer_Something* vs=(Viewer_Something*)(n->value);
    Viewer_Faces*vf=(Viewer_Faces*)(vs->evolution);
    vf->Data_index_rows=node_size(node);
    Int_RB_Tree* tree=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(tree);
    int f_size=0, i=0;
    for(Node* nit=node;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c* c=(template_c*)(nit->value);
        f_size+=c->vertices_size+1;
        for(int j=0;j<c->vertices_size;j++)
        {
            if(tree->find(tree,c->vertices[j]->id)==NULL)
            {
                int * value=(int*)malloc(sizeof(int));
                *value=i;
                tree->insert(tree,c->vertices[j]->id,value);
                i++;
            }
        }
    }
    vf->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*f_size);
    vf->Data_rows=i; 
    vf->Data=(float*)malloc(sizeof(float)*3*vf->Data_rows);

    i=0;
    for(Node* nit=node;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c* c=(template_c*)(nit->value);
        vf->Data_index[i]=c->vertices_size;
        i++;
        for(int j=0;j<c->vertices_size;j++)
        {
            vf->Data_index[i]=*((int*)(tree->find(tree,c->vertices[j]->id)));
            int k=vf->Data_index[i];
            vf->Data[k*3+0]=c->vertices[j]->point[0];vf->Data[k*3+1]=c->vertices[j]->point[1];vf->Data[k*3+2]=c->vertices[j]->point[2];
             
            i++;  
        } 
    }    
    int_rb_tree_free_value(tree);
    int_rb_tree_free(tree);

    vf->color_rows=vf->Data_index_rows;
    vf->set_color(vf,0.7,0.6,0.7,1.0);

    vf->normal_rows=vf->Data_index_rows;
    vf->compute_normal(vf);

    free_node(n);
}

static void test_show_mesh_cells_from_mesh(Viewer_World* vw,Mesh*m)
{
    Node* n=vw->create_something(vw,"Faces");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Faces* vf=(Viewer_Faces*)(vs->evolution);
     
    vf->Data_rows=m->num_v(m);
    vf->Data_index_rows=m->num_c(m);
    get_data_from_2dim_cell(m,&(vf->Data),&(vf->Data_index));
    
    
    vf->color_rows=vf->Data_index_rows;
    vf->set_color(vf,0.4,0.6,0.7,1.0);

    vf->normal_rows=vf->Data_index_rows;
    vf->compute_normal(vf);
    free_node(n); 
}



void test_show_mesh_lines(Viewer_World* vw,Mesh* m)
{
    Node* n=vw->create_something(vw,"Edges");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Edges* ve=(Viewer_Edges*)(vs->evolution);
    ve->Data_rows=m->num_v(m);
    ve->Data_index_rows=m->num_f(m);
    get_lines_data_from_2dim_cell(m,&(ve->Data),&(ve->Data_index));
    ve->color_rows=ve->Data_index_rows;
    ve->set_color(ve,0.9,0.5,0.5,1.0); 
    free_node(n);
}

static inline double *out_product(double *a,double*b)
{
    double* re=(double*)malloc(sizeof(double)*3);
    
    re[0]=a[1]*b[2]-a[2]*b[1];
    re[1]=-a[0]*b[2]+a[2]*b[0];
    re[2]=a[0]*b[1]-a[1]*b[0];
    return re; 
}
static inline double inner_product(double* a,double* b)
{
     return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}
static inline int normalize(double *a)
{
    double norm=a[0]*a[0]+a[1]*a[1]+a[2]*a[2];
    
    norm=SAFE_SQRT(norm);
    //1e-12 doen't affect the result
    if(norm==0||fabs(norm)<1e-11)
    {
        return 0;
    } 
    a[0]/=norm;a[1]/=norm;a[2]/=norm;
    return 1;
}


static inline double*get_triangle_plane_equation(double* tr1,double* tr2,double*tr3)
{
    double a[3]={0},b[3]={0};
    a[0]=tr2[0]-tr1[0];
    a[1]=tr2[1]-tr1[1];
    a[2]=tr2[2]-tr1[2];

    b[0]=tr3[0]-tr2[0];
    b[1]=tr3[1]-tr2[1];
    b[2]=tr3[2]-tr2[2];
    double * n=out_product(a,b);
    if(!normalize(n))
    {
        free(n);
        return NULL;
    }
    double * re=(double*)malloc(sizeof(double)*4);
    re[3]=-(n[0]*tr1[0]+n[1]*tr1[1]+n[2]*tr1[2]);
    re[0]=n[0];re[1]=n[1];re[2]=n[2];
    // printf("test :%lf\n",re[0]*tr1[0]+re[1]*tr1[1]+re[2]*tr1[2] );
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] ); 
    // printf("test :%lf\n",re[0]*tr2[0]+re[1]*tr2[1]+re[2]*tr2[2] ); 
    
    free(n); 
    return re; 
}



Matrix4x4* get_matrix_q_from_cell(template_c* c)
{
    Matrix4x4* re=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_double(re);

    double * plane=get_triangle_plane_equation(c->vertices[0]->point,c->vertices[1]->point,c->vertices[2]->point);
    double* data=(double*)(re->data);
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            data[i*4+j]=plane[i]*plane[j];
        }
    }
    SAFE_FREE(plane);
    return re; 
}

Int_RB_Tree* compute_matrix_q_every_cell(Mesh* m)
{
    Int_RB_Tree* re=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(re);
    for(auto cit=m->c_begin(m);cit!=m->c_end(m);cit++)
    {
        Matrix4x4* mat=get_matrix_q_from_cell(quote(cit));
        re->insert(re,quote(cit)->id,mat); 
    }

    return re;
}


//m= m1+m2
Matrix4x4* matrix_add(Matrix4x4* m1,Matrix4x4* m2)
{
    Matrix4x4* re=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_double(re); 
   
    double* data=(double*)(re->data),*data1=(double*)(m1->data),*data2=(double*)(m2->data);
    for(int i=0;i<4;i++)
    {
        for(int j=0;j<4;j++)
        {
            data[i*4+j]=data1[i*4+j]+data2[i*4+j];
        }
    }
    return re;
}


Matrix4x4 * get_matrix_q_from_vertex(template_v* v,Int_RB_Tree* tree)
{
    Matrix4x4* re=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_double(re);
    re->zero(re);
    double* data=(double*)(re->data);
    for(Node* nit=v->cells;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c* c=(template_c*)(nit->value);
        //Matrix4x4* m=get_matrix_q_from_cell(c);
        Matrix4x4* m=(Matrix4x4*)tree->find(tree,c->id);
        double* data1=(double*)(m->data); 
        for(int i=0;i<4;i++)
        {
            for(int j=0;j<4;j++)
            {
                data[i*4+j]+=data1[i*4+j];
            }
        }
        //Matrix4x4_free(m);
    }

    return re;
}

static inline int is_edge_legacy_to_collapse(Mesh* m,template_f* f)
{
    template_v* v1=f->vertices[0],*v2=f->vertices[1];
    Node*n1= m->vv_begin(m,*v1),*n2=m->vv_begin(m,*v2);
    Node* n=node_intersection(n1,n2);
    int size=node_size(n);

    free_node(n);free_node(n1);free_node(n2);

    return size;
}
static inline double two_point_distance(double*p1,double*p2)
{
    double p[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};
    return SAFE_SQRT(p[0]*p[0]+p[1]*p[1]+p[2]*p[2]);
}

//每个点的矩阵属性

Int_RB_Tree* qem_init_qi(Mesh * m)
{

    Int_RB_Tree* re=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(re);
    Int_RB_Tree* tree= compute_matrix_q_every_cell(m);
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        Matrix4x4* m= get_matrix_q_from_vertex(quote(vit),tree);
        re->insert(re,quote(vit)->id,m);
    }
    for(auto it=tree->begin(tree);it.it!=NULL;it++)
    {
        Matrix4x4_free((Matrix4x4*)(it.second));
    }
    int_rb_tree_free(tree);
    return re;
}


static inline double*  compute_qem_one_edge_position(Mesh* m,template_f* f,Int_RB_Tree *tree)
{
    double* p=NULL;
    template_v* v1=f->vertices[0],*v2=f->vertices[1];
    Matrix4x4* m1=(Matrix4x4*)tree->find(tree,v1->id);
    Matrix4x4* m2=(Matrix4x4*)tree->find(tree,v2->id);
    Matrix4x4* m3=matrix_add(m1,m2);
    double* data=(double*)(m3->data);
    data[12]=0;data[13]=0;data[14]=0;data[15]=1;
    Matrix4x4* mi=m3->inverse(m3); 
    double mid_p[3]={(v1->point[0]+v2->point[0])/2.0,(v1->point[1]+v2->point[1])/2.0,(v1->point[2]+v2->point[2])/2.0};

    if(mi!=NULL)
    {
        double* datami=(double*)(mi->data);
        p=(double*)malloc(sizeof(double)*3);
        memset(p,0,sizeof(double)*3);
        p[0]=datami[0*4+3];p[1]=datami[1*4+3];p[2]=datami[2*4+3];
        if(two_point_distance(mid_p,p)>2*two_point_distance(mid_p,v1->point))
        {
            p[0]=mid_p[0];p[1]=mid_p[1];p[2]=mid_p[2];
        }
        Matrix4x4_free(mi);
    } 
    else
    {
        p= (double*)malloc(sizeof(double)*3);
        memset(p,0,sizeof(double)*3);
        p[0]=mid_p[0];p[1]=mid_p[1];p[2]=mid_p[2];
    } 
    Matrix4x4_free(m3);
    return p;
}

static double compute_xTAx(Matrix4x4* Q,double* p)
{
    double* data=(double*)(Q->data);
    double p1[4]={0,0,0,0},p2[4]={p[0],p[1],p[2],1};
    for(int i=0;i<4;i++)
    {
        double temp=0;
        for(int j=0;j<4;j++)
        {
            temp+=data[i*4+j]*p2[j];
        }
        p1[i]=temp;
    }
    return (p2[0]*p1[0]+p2[1]*p1[1]+p2[2]*p1[2]+p2[3]*p1[3]);
}

// tree是每个边对应的点坐标
// tree1是每个点对应的q矩阵
//
static inline double qem_compute_one_edge_cost(template_f*f ,Int_RB_Tree*tree,double* p)
{
    if(p==NULL)
    {
        return -1;
    }
    Matrix4x4* m1=(Matrix4x4*)(tree->find(tree,f->vertices[0]->id));
    Matrix4x4* m2=(Matrix4x4*)(tree->find(tree,f->vertices[1]->id));

    Matrix4x4 *m3=matrix_add(m1,m2);
    if(f->id==1564574)
    {
        
    }
    double re= compute_xTAx(m3,p);
    Matrix4x4_free(m3);

    return re; 
}
typedef struct Edge_Energy{
    double energy;
    int fid;
    double* p;
    void* prop;
}Edge_Energy;

static inline void edge_energy_init(Edge_Energy* ee)
{
    ee->energy=0;
    ee->p=NULL;
    ee->fid=-1;
    ee->prop=NULL;
}
static inline void edge_energy_free(Edge_Energy*ee)
{
    if(ee==NULL)
    {
        return;
    }
    SAFE_FREE(ee->p);
    free(ee);
}

static int edge_energy_compare(const void* v1,const void* v2)
{
    Edge_Energy*ee1=*((Edge_Energy**)(v1)),*ee2=*((Edge_Energy**)(v2));
    return ee1->energy>ee2->energy?1:(ee2->energy>ee1->energy?(-1):0);
}


CHeap *compute_qem_all_edge_position_and_energy(Mesh* m,Int_RB_Tree*tree)
{
    CHeap *ch=(CHeap*)malloc(sizeof(CHeap));
    cheap_init(ch);
    Edge_Energy** values=(Edge_Energy**)malloc(sizeof(Edge_Energy)*(m->num_f(m)+500000));
    memset(values,0,sizeof(Edge_Energy)*(m->num_f(m)+500000));
    ch->values=values;
    ch->size=0;
    ch->width=sizeof(void*);
    ch->comp=edge_energy_compare;

    for(auto fit=m->f_begin(m);fit!=m->f_end(m);fit++)
    {
        int size=is_edge_legacy_to_collapse(m,quote(fit));
        if(size==2)
        {
            double* p=compute_qem_one_edge_position(m,quote(fit),tree);
            if(p==NULL)
            {
                continue;
            }
            double energy=qem_compute_one_edge_cost(quote(fit),tree,p); 
            // if(quote(fit)->id==1564574)
            // {
            //     printf("v1:%d v2:%d energy:%lf \n",quote(fit)->vertices[0]->id,quote(fit)->vertices[1]->id,energy); 
                
            // }
            Edge_Energy* data=(Edge_Energy*)malloc(sizeof(Edge_Energy));
            edge_energy_init(data);
            data->energy=energy;
            data->p=p;
            data->fid=quote(fit)->id;
            values[ch->size]=data;
            ch->size=ch->size+1;
        } 
        else if(size>2)
        {
           // printf("is not legacy\n");
        }
        else
        {
            //printf("is boundary\n"); 
        }
    } 
   
    make_cheap(ch);

    return ch; 
}
void qem_collapse_edge(Mesh *m ,Edge_Energy*ee,Int_RB_Tree* tree_v,CHeap*ch)
{
    if(ee==NULL)
    {
        printf("cuou\n");
        return;
    }
    template_f* f=m->get_facep(m,ee->fid);
    if(f==NULL||is_edge_legacy_to_collapse(m,f)!=2)
    {
        printf("cuowu1\n");
        return;
    } 
    template_v* v1=f->vertices[0],*v2=f->vertices[1],
        *n_v=m->create_vertexv(m,ee->p,3);
    Matrix4x4* m1=(Matrix4x4*)(tree_v->find(tree_v,v1->id));
    Matrix4x4* m2=(Matrix4x4*)(tree_v->find(tree_v,v2->id));
    Matrix4x4* m3=matrix_add(m1,m2);
    tree_v->erase(tree_v,v1->id);
    tree_v->erase(tree_v,v2->id); 

    tree_v->insert(tree_v,n_v->id,m3);

    Matrix4x4_free(m1);Matrix4x4_free(m2);
    Int_RB_Tree* cells=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(cells); 
   

    for(auto vcit=m->vc_begin(m,*v1);vcit!=m->vc_end(m,*v1);vcit++)
    {
        cells->insert(cells,quote(vcit)->id,quote(vcit)); 
    }
    for(auto vcit=m->vc_begin(m,*v2);vcit!=m->vc_end(m,*v2);vcit++)
    {
        cells->insert(cells,quote(vcit)->id,quote(vcit));  
    }

    Node* boundary_hfaces=NULL; 
    for(auto it=cells->begin(cells);it.it!=NULL;it++)
    {
        template_c* c=(template_c*)(it.second);
        for(auto chfit=m->chf_begin(m,*c);chfit!=m->chf_end(m,*c);chfit++)
        {
            template_hf* hf=quote(chfit);
            if(hf->vertices[0]!=v1&&hf->vertices[0]!=v2&&hf->vertices[1]!=v1&&hf->vertices[1]!=v2)
            {
                Node *hfs=NULL;
                hfs=node_overlying(hfs,hf->vertices[1]);
                hfs=node_overlying(hfs,hf->vertices[0]);
                boundary_hfaces=node_overlying(boundary_hfaces,hfs);
            }
           
        }  
    }

    for(auto it=cells->begin(cells);it.it!=NULL;it++)
    {
        template_c* c=(template_c*)(it.second);
        if(!m->delete_cell(m,*c,true))
        {
            printf("cuowu\n");
        }
    }
    for(Node*nit=boundary_hfaces;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v*vs[3]={NULL,NULL,n_v};
        int i=0;
        for(Node*nit1=(Node*)(nit->value);nit1!=NULL;nit1=(Node*)(nit1->Next))
        {
            vs[i]=(template_v*)(nit1->value);
            i++;
        }
        template_c* c=m->create_cellv(m,vs,3);
        if(c==NULL)
        {
            printf("cuowu\n");
        }
        free_node((Node*)(nit->value));
    }
    free_node(boundary_hfaces);
    
   

    for(auto vfit=m->vf_begin(m,*n_v);vfit!=m->vf_end(m,*n_v);vfit++)
    {
        double* p=compute_qem_one_edge_position(m,quote(vfit),tree_v);
        if(p==NULL){
            continue;
        }
        double energy=qem_compute_one_edge_cost(quote(vfit),tree_v,p); 
        Edge_Energy* data=(Edge_Energy*)malloc(sizeof(Edge_Energy));
        edge_energy_init(data);
        data->energy=energy;
        data->p=p;
        data->fid=quote(vfit)->id;
        insert_cheap(ch,&data);
    }
    int_rb_tree_free(cells);     
}


void test1()
{
    Mesh mesh;
    Mesh_init(&mesh);
    _ReadOff_(&mesh,"bunny.off",3);
    Viewer_World_Manager vwm;
    viewer_world_manager_init(&vwm);
    Viewer_World *vw=vwm.create_world(&vwm,NULL);
    add_default_somethings(vw);
    Int_RB_Tree*tree= qem_init_qi(&mesh);
    
    CHeap* ch= compute_qem_all_edge_position_and_energy(&mesh,tree);
    if(*((Edge_Energy**)(ch->values+ch->size*ch->width))!=NULL)
    {
        printf("zheli cuowu\n");
    }
    printf("size:%d\n",ch->size);
    for(int i=0;i<1;i++)
    {
        void* value=pop_cheap(ch);
        Edge_Energy*ee=*((Edge_Energy**)(value));
       
        printf("%d\n",ch->size);

        qem_collapse_edge(&mesh,ee,tree,ch);
        edge_energy_free(ee);
        free(value);
    }
    printf("num v:%d num_c :%d\n",mesh.num_v(&mesh),mesh.num_c(&mesh));
    test_show_mesh_cells_from_mesh(vw,&mesh);
    Mesh_free(&mesh);


    Viewer_Opengl_Interpreter voi;
    viewer_opengl_interpreter_initn(&voi,&vwm);
    voi.interpreter(&voi);
    viewer_opengl_interpreter_free(&voi);
    viewer_world_manager_free(&vwm);
}



int main()
{
    
    test1();

    printf("end\n");

    return 0;
}
